#include "inc/hw_types.h"		// tBoolean
#include "inc/hw_memmap.h"
#include "RASLib/init.h"
#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"

#include "RASLib/motor.h"
#include "RASLib/encoder.h"
#include "RASLib/servo.h"
#include "RASLib/uart.h"	
#include "RobzDemo.h"
 
void initMotors(void);
void initLineSensor(void);
void initUART(void);
void initIRSensor(void);
void initEncoders(void);
void Wait(long ms);
void initServo(void);
void SetDischargeTime(int ms);
long getADCValue(void);
unsigned char ReadLineSensor(void);
void UARTprintf(const char *pcString, ...);
void PIDwallFollow(long position);
void initAll_WF(void);
void turn(tBoolean);
long abs(long);

/**
 *Since our last attempt failed, I thought we should start from scratch and have
 *everything called in fucntions
 */

long WF_distance;
long WF_Kp=5,WF_Ki=0,WF_Kd=27;	   //15 0 27
long WF_currentError,WF_lastError;
long WF_integral=0;		 
long MVabs=0;
signed char velocity; 
signed char speed=127; 
signed long encoder0, encoder1;	

//Above 8.09 V = 107		
long encoderTurn=115;//115
long encoderTurn2=100;
char turnRadius=5;
long rightTurnDistances=500;

void wallFollow2(){
	//Encoders
	int turned=0;
	int i=0;

	long IRvalue;
	unsigned char lineSensor;
	long noWall=90;

	//Random variables 	  
	unsigned char servoPosition=20,servoPosition2=141;

	initAll_WF();		
						  
	SetServoPosition(SERVO_0, servoPosition);
	Wait(2000);			   
	SetMotorPowers(speed,speed-motorOffset);
	WF_distance=getADCValue(); 
	//Wait(10);

	while(1){
		//PRINT_D(i++); NL;
		IRvalue=getADCValue();	 
		//Wait(10);
		lineSensor=ReadLineSensor();
		//UARTprintf("%u        \r",lineSensor);
		if(WF_distance-IRvalue>noWall&&turned<3)turned++;  
		if(turned==3&&abs(WF_distance-IRvalue)>=noWall){				//This is the first turn (left)
			turn(true);	//turns left	  
	 		WF_distance=getADCValue(); 
			//Wait(10);
			turned++;
		}
		else if(turned==4&&lineSensor!=255){					//This sooks for the wall
			SetServoPosition(SERVO_0,servoPosition2);
			Wait(500);
			turned++;
		}
		else if(turned==5&&IRvalue>=rightTurnDistances){				//This is the second turn (right)
			encoderTurn+=18;
			turn(false);	//turns right
			turned++;
			SetMotorPowers(0,0);
			break;
		}

		GetEncoderCounts(&encoder0, &encoder1);
		
		PIDwallFollow(IRvalue);  
	}
}

void PIDwallFollow(long position){
	//PID variables 	 
	signed long derivative=0;
    signed long Pout;
	signed long Iout;
	signed long Dout;
	signed long MV=0;
	
	WF_lastError=WF_currentError;
	//Now it's time to compute the error...
	WF_currentError = (position - WF_distance);
	WF_currentError/=20;
	//Now that we have the error, we can update the derivative and integral
	derivative=WF_currentError-WF_lastError;
	WF_integral+=WF_currentError;
	//It is now time to compute Pout, Iout, and Dout, thus getting MV (the manipulated variable)
	//It should be noted that a positive derivative means the robot needs to turn left, and a
	//negative derivative means the robot needs to turn right. A positive integral means the
	//robot needs to skew left, and a negative integral value means the robot needs to skew right
	Pout=WF_Kp*WF_currentError;								   
	Dout=WF_Kd*derivative;
	Iout=WF_Ki*WF_integral/20;
	MV=Pout+Iout+Dout;
	
	if(MV>255)MV=255;
	if(MV*-1>255)MV=-255;
	//If MV is positive, the robot needs to go left. If MV is negative, the robot needs to go right
	//But... we first need to come up with a linear relationship relating MV to the speed of the
	//reduced wheel (This means an mx+b relationship... more like a b-mx relationship)
	MVabs=MV;
	if(MVabs<0)MVabs*=-1;
	velocity=(signed char)(speed-MVabs);
	if(velocity<-128)velocity=-128;//not possible??	
	if(MV>0){
		SetMotorPowers(speed,velocity);
	}
	else if(MV<0){
		SetMotorPowers(velocity,speed-motorOffset);
	}
	else{//MV equals 0; never going to happen, but might?
		SetMotorPowers(speed,speed-motorOffset);
	}		 
}

void initAll_WF(){
	initIRSensor();
	initEncoders();
	initMotors();
	initLineSensor();
	initServo();
	SetDischargeTime(220);
	PresetEncoderCounts(0,0);
}

void turn(tBoolean turnLeft){			   
	PresetEncoderCounts(0,0);
	if(turnLeft)SetMotorPowers(turnRadius,127); 
	else SetMotorPowers(127,-22);
	GetEncoderCounts(&encoder0, &encoder1);
	if(turnLeft){
		while(encoder0<encoderTurn&&encoder1<encoderTurn){
			GetEncoderCounts(&encoder0, &encoder1);
		}
	}
	else{
		while(encoder0<encoderTurn2&&encoder1<encoderTurn2){
			GetEncoderCounts(&encoder0, &encoder1);
		}
	}
	if(turnLeft){	
		SetMotorPowers(127,127-motorOffset);
		Wait(2000);
	}
	//WF_Kp=0;
}

long abs(long value){
	if(value<0)value*=-1;
	return value;
}
